clear all;
close all;
xk=[0;0];
nu1=0;nu2=0;
T=0.001;ttt=0;flag=0;
for k=1:1:4000
time(k)=k*T;
ttt=time(k);
if time(k)<1.2
    yd(k)=5/6*time(k)/1000;
elseif time(k)<2.2
    yd(k)=(3.4-2*time(k))/1000;
elseif time(k)<3
    yd(k)=(-6.5+2.5*time(k))/1000;
elseif time(k)<=4
    yd(k)=(7-2*time(k))/1000;
end

if time(k)<1.2
    dyd(k)=5/6/1000;
elseif time(k)<2.2
    dyd(k)=-2/1000;
elseif time(k)<3
    dyd(k)=2.5/1000;
elseif time(k)<=4
    dyd(k)=-2/1000;
end

N=2;
if N==1
    d(k)=0;
elseif N==2
    fmax=0.125; 
    fc=8;  
    mt(k)=40*sin(2*pi*fmax*k*T);
    d(k)=mt(k)*sin(2*pi*fc*k*T);
end

tSpan=[0 T];

para1=[nu1,nu2];      % D/A
[tt,xx]=ode45('plant',tSpan,xk,[],para1);
xk=xx(length(xx),:);    % A/D
y(k)=xk(1);
dy(k)=xk(2);

m=3.31;kv=8.6;kc=11.5;
lame=0.016;gama=1.4;epsilon=0.00002;


e=y(k)-yd(k);
de=dy(k)-dyd(k);
s=e+lame*((abs(de))^gama)*sign(de);
f=kv*dy(k)+kc*sign(dy(k));
k0=abs(s)/(epsilon-abs(s));


u0=f-m*((lame*gama)^(-1))*((abs(de))^(2-gama))*sign(de);
u1=-m*k0*sign(s);
ut(k)=u0+u1;

nu1=ut(k);
nu2=d(k);
end
figure(1);
plot(time,yd,'k',time,y,'b:','linewidth',2);
legend('Ideal position signal','Position tracking');
xlabel('time(s)');ylabel('Angle response');

figure(2);
plot(time,yd-y,'k','linewidth',2);
xlabel('time(s)');ylabel('Trscking Error');

figure(3);
plot(time,ut,'k');
legend('CONTROL');
xlabel('time(s)');ylabel('Control input');
figure(4);
plot(time,d,'k');
legend('CONTROL');
xlabel('time(s)');ylabel('Control input');